from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
import os

from ament_index_python.packages import get_package_share_directory
def generate_launch_description():

    bringup_dir = get_package_share_directory('point_cloud_show')
    params_file = os.path.join(bringup_dir, 'launch', 'segmentation_params_mapping.yaml')
    # Nodes launching commands
    ground_segmentation_cmd = Node(
            package='linefit_ground_segmentation_ros',
            executable='ground_segmentation_node',
            output='screen',
            parameters=[params_file])
    
    point_cloud_show_node = Node(
        package='point_cloud_show',
        executable='point_cloud_show',
        name='point_cloud_show',
    )

    rviz_node = Node(
    package='rviz2',
    executable='rviz2',
    name='rviz2',
    output='screen',
    arguments=['-d', os.path.join(bringup_dir, 'rviz', 'mapping.rviz')],
    )
    
    global_planner_cmd=Node(
            package='global_planner',
            executable='global_planner',
            name='global_planner',
            parameters=[{'set_node': True}],
            output='screen'
    )
    

    return LaunchDescription([
        point_cloud_show_node,
        rviz_node,
        ground_segmentation_cmd,
        global_planner_cmd
    ])

if __name__ == '__main__':
    generate_launch_description()